
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       pid_2d.c
  * @author     baiyang
  * @date       2021-8-8
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <stdbool.h>

#include "pid_2d.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// Constructor
void pid_2d_ctrl_ctor(PID_2d_ctrl * controler, float kp, float ki, float kd, float kff, float kimax, float filt_hz, float filt_d_hz, float initial_dt)
{
    controler->_kp = kp;
    controler->_ki = ki;
    controler->_kd = kd;
    controler->_kff = kff;
    controler->_kimax = fabsf(kimax);
    controler->_filt_E_hz = fabsf(filt_hz);
    controler->_filt_D_hz = fabsf(filt_d_hz);

    controler->_dt = initial_dt;

    // reset input filter to first value received
    pid_2d_ctrl_reset_filter(controler);
}

//  update_all - set target and measured inputs to PID controller and calculate outputs
//  target and error are filtered
//  the derivative is then calculated and filtered
//  the integral is then updated if it does not increase in the direction of the limit vector
Vector2f_t pid_2d_ctrl_update_all(PID_2d_ctrl * controler, const Vector2f_t* target, const Vector2f_t* measurement, const Vector2f_t* limit)
{
    Vector2f_t output = {0};

    // don't process inf or NaN
    if (vec2_is_nan(target) || vec2_is_inf(target) ||
        vec2_is_nan(measurement) || vec2_is_inf(measurement)) {
        return output;
    }

    controler->_target = *target;

    // reset input filter to value received
    if (controler->_reset_filter) {
        controler->_reset_filter = false;
        vec2_sub(&controler->_error, &controler->_target, measurement);
        vec2_zero(&controler->_derivative);
    } else {
        Vector2f_t error_last = {controler->_error.x, controler->_error.y};
        Vector2f_t t1 = {controler->_target.x - measurement->x, controler->_target.y - measurement->y};
        Vector2f_t t2 = {t1.x - controler->_error.x, t1.y - controler->_error.y};

        vec2_mult(&t2, &t2, pid_2d_ctrl_get_filt_E_alpha(controler));
        vec2_add(&controler->_error, &controler->_error, &t2);

        // calculate and filter derivative
        if (controler->_dt > 0.0f) {
            const Vector2f_t derivative = {(controler->_error.x - error_last.x) / controler->_dt,
                                           (controler->_error.y - error_last.y) / controler->_dt};

            Vector2f_t t3 = {derivative.x - controler->_derivative.x, derivative.y - controler->_derivative.y};
            
            vec2_mult(&t1, &t1, pid_2d_ctrl_get_filt_D_alpha(controler));
            vec2_add(&controler->_derivative, &controler->_derivative, &t3);
        }
    }

    // update I term
    pid_2d_update_i(controler, limit);

    output.x = controler->_error.x * controler->_kp + controler->_integrator.x + controler->_derivative.x * controler->_kd + controler->_target.x * controler->_kff;
    output.y = controler->_error.y * controler->_kp + controler->_integrator.y + controler->_derivative.y * controler->_kd + controler->_target.y * controler->_kff;
    
    return output;
}

Vector2f_t pid_2d_update_all2(PID_2d_ctrl * controler, const Vector3f_t *target, const Vector3f_t *measurement, const Vector3f_t *limit)
{
    Vector2f_t _target = {target->x, target->y};
    Vector2f_t _measurement = {measurement->x, measurement->y};
    Vector2f_t _limit = {limit->x, limit->y};
    
    return pid_2d_ctrl_update_all(controler, &_target, &_measurement, &_limit);
}

Vector2f_t pid_2d_update_all3(PID_2d_ctrl * controler, const Vector3f_t *target, const Vector2f_t *measurement, const Vector3f_t *limit)
{
    Vector2f_t _target = {target->x, target->y};
    Vector2f_t _limit = {limit->x, limit->y};
    
    return pid_2d_ctrl_update_all(controler, &_target, measurement, &_limit);
}

//  update_i - update the integral
//  If the limit is set the integral is only allowed to reduce in the direction of the limit
void pid_2d_update_i(PID_2d_ctrl * controler, const Vector2f_t *limit)
{
    Vector2f_t limit_direction = *limit;
    Vector2f_t delta_integrator = {(controler->_error.x * controler->_ki) * controler->_dt,
                                   (controler->_error.y * controler->_ki) * controler->_dt};

    if (!math_flt_zero(vec2_length_squared(&limit_direction))) {
        // zero delta_vel if it will increase the velocity error
        vec2_norm(&limit_direction, &limit_direction);
        if (math_flt_positive(vec2_dot(&delta_integrator, limit))) {
            vec2_zero(&delta_integrator);
        }
    }

    vec2_add(&controler->_integrator, &controler->_integrator, &delta_integrator);
    vec2_limit_length(&controler->_integrator, controler->_kimax);
}

// get_pi - get results from pid controller
Vector2f_t pid_2d_ctrl_get_pid(PID_2d_ctrl * controler)
{
    
    Vector2f_t pid = {controler->_error.x * controler->_kp + controler->_integrator.x + controler->_derivative.x * controler->_kd,
                      controler->_error.y * controler->_kp + controler->_integrator.y + controler->_derivative.y * controler->_kd};

    return pid;
}

Vector2f_t pid_2d_ctrl_get_p(PID_2d_ctrl * controler)
{
    Vector2f_t p = {controler->_error.x * controler->_kp,
                    controler->_error.y * controler->_kp};
    return p;
}

Vector2f_t pid_2d_ctrl_get_d(PID_2d_ctrl * controler)
{
    Vector2f_t d = {controler->_derivative.x * controler->_kd,
                    controler->_derivative.y * controler->_kd};
    return d;
}

Vector2f_t pid_2d_ctrl_get_ff(PID_2d_ctrl * controler)
{
    controler->_error.x *= controler->_kff;
    controler->_error.y *= controler->_kff;

    return controler->_error;
}

// reset_I - reset the integrator
void pid_2d_ctrl_reset_I(PID_2d_ctrl * controler)
{
    controler->_integrator.x = 0;
    controler->_integrator.y = 0;
}

// reset_filter - input and D term filter will be reset to the next value provided to set_input()
void pid_2d_ctrl_reset_filter(PID_2d_ctrl * controler)
{
    controler->_reset_filter = true;
    controler->_derivative.x = 0.0f;
    controler->_derivative.y = 0.0f;

    controler->_integrator.x = 0.0f;
    controler->_integrator.y = 0.0f;

}

void pid_2d_set_integrator4(PID_2d_ctrl * controler, const Vector2f_t* target, const Vector2f_t* measurement, const Vector2f_t* i)
{
    Vector2f_t _error = {target->x - measurement->x, target->y - measurement->y};

    pid_2d_set_integrator3(controler, &_error, i);
}

void pid_2d_set_integrator3(PID_2d_ctrl * controler, const Vector2f_t* error, const Vector2f_t* i)
{
    Vector2f_t _i = {i->x - error->x * controler->_kp, i->y - error->y * controler->_kp};

    pid_2d_set_integrator(controler, &_i);
}

void pid_2d_set_integrator(PID_2d_ctrl * controler, const Vector2f_t* i)
{
    controler->_integrator = *i;
    const float integrator_length = vec2_length(&controler->_integrator);
    if (integrator_length > controler->_kimax) {
        vec2_mult(&controler->_integrator, &controler->_integrator, (controler->_kimax / integrator_length));
    }
}
/*------------------------------------test------------------------------------*/


